/* 
 * Copyright 2011 kOSMik.
 * This file is part of kOSMik.
 * 
 * kOSMik is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * kOSMik is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with kOSMik.  If not, see <http://www.gnu.org/licenses/>.
 *  
 * kOSMik:
 * Open Street Map - Editor.
 * http://code.google.com/p/swp-dv-ws2010-osm-2/
 */

package de.fuberlin.kosmik.filter;

import static java.lang.Math.pow;
import Jama.Matrix;
import android.location.Location;
import android.location.LocationManager;
import android.util.Log;
import de.fuberlin.kosmik.log.Constants;

/**
 * Smoothing filter based on a kalman filter.
 * 
 * @author Michael Schmidt
 */
public class SmoothingFilter implements Filter {

	// process parameters
	private static final double DT = 20d;
	private static final double PROCESS_NOISE = 2d;
	private static final double MEASUREMENT_NOISE = 600d;
	private static final float FILTER_MAX_DISTANCE = 6f;
	private static boolean filterUsage = false;

	private KalmanFilter kf;

	/**
	 * Construct new smoothing filter based on a kalman filter.
	 */
	public SmoothingFilter() {
		// nothing to do here
	}

	@Override
	public Location filter(Location location) {

		if (kf == null) {
			// initiate filter
			Log.d(Constants.TAG, "Init filter ...");
			kf = KalmanFilter.buildKF(location.getLatitude(), location.getLongitude(), DT,
					pow(PROCESS_NOISE, 2) / 2, pow(MEASUREMENT_NOISE, 2));
		}

		Location loc = new Location(LocationManager.GPS_PROVIDER);

		do {
			kf.predict();
			kf.correct(new Matrix(new double[][] { { location.getLatitude() },
					{ location.getLongitude() } }));

			loc.setLatitude(kf.getX().get(0, 0));
			loc.setLongitude(kf.getX().get(1, 0));

			// justify filter by feed him with the first position, until
			// he predicts a position next to our GPS fix
		} while (loc.distanceTo(location) > FILTER_MAX_DISTANCE && !filterUsage);

		// if distance is too big ignore filter
		if (loc.distanceTo(location) > FILTER_MAX_DISTANCE)
			return location;

		filterUsage = true;
		Log.d(Constants.TAG, "Filter lat: " + loc.getLatitude() + " lng: " + loc.getLongitude());

		return loc;
	}
}
